#include "StdAfx.h"
#include "RobotExperiment.h"
#include "OgreUtils.h"

RobotExperiment::RobotExperiment(World* world):
world(world)
{
	robot.reset(new Robot(world));
}

float* RobotExperiment::getNeuralWeights()
{
	return robot->getNeuralWeights();
}

size_t RobotExperiment::getNeuralWeightsCount()
{
	return robot->getNeuralWeightsCount();
}

float RobotExperiment::getFitness()
{
	reset(false);

	const int T = 800;
	float t = 0.f;
	for(int i = 0; i < T; i++)
	{
		update(false);
		btVector3 aabbMin;
		btVector3 aabbMax;
		robot->getBody()->getAabb(aabbMin, aabbMax);
		float h = aabbMin.y();
		if(h < 10.f) break;
		t += 1.f;
	}

	btVector3 aabbMin;
	btVector3 aabbMax;
	robot->getBody()->getAabb(aabbMin, aabbMax);
	float pos = std::max(0.f, aabbMin.z());
	
	float f = pos + 0.001f*t;
	return f;
}

void RobotExperiment::update(bool syncPhysicsAndGraphics)
{
	robot->update();
	world->update(syncPhysicsAndGraphics);
}

void RobotExperiment::reset(bool syncPhysicsAndGraphics)
{
	world->reset(syncPhysicsAndGraphics);
}